//
// Created by cch on 22-5-2.
//
#include "initalizer.h"
#include "utils.h"

void IMUGPS::Initializer::AddImuData(const ImuDataPtr &imuDataPtr) {

  imu_buffer.push_back(imuDataPtr);
  if(imu_buffer.size() > 100)
  {
    imu_buffer.pop_front();
  }
}

bool IMUGPS::Initializer::AddGpsData(const GpsDataPtr& gpsDataPtr, State *state) {
    if(imu_buffer.size() < 100)
    {
        return false;
    }
    const ImuDataPtr last_imu_Ptr = imu_buffer.back();
    //如果时间戳过大则放弃
    if(std::abs(gpsDataPtr->timestamp - last_imu_Ptr->timestamp) > 0.5)
    {
      return false;
    }
    state->timestamp = last_imu_Ptr->timestamp;
    state->imuDataPtr = last_imu_Ptr;

    //状态初始化
    state->G_R_I = Eigen::Matrix3d::Identity();
    state->G_V_I.setZero();
    //如果外参没算出来，则放弃
    if(!ComputeRotationFromImu(state->G_R_I))
    {
      return false;
    }
    //设置陀螺仪偏置
    state->acc_bias.setZero();
    state->gyro_bias.setZero();
    //设置协方差
    state->cov.setZero();
    state->cov.block<3,3>(0,0) = 100 * Eigen::Matrix3d::Identity();
    state->cov.block<3,3>(3,3) = 100 * Eigen::Matrix3d::Identity();
    state->cov.block<2,2>(6,6) = 10 * IMUGPS::kDegreeToRadian * 10 * IMUGPS::kDegreeToRadian * Eigen::Matrix2d::Identity();
    state->cov(8,8) = 100 * kDegreeToRadian * 100 * kDegreeToRadian;
    state->cov.block<3,3>(9,9) = 0.0004 * Eigen::Matrix3d::Identity();
    state->cov.block<3,3>(12,12) = 0.0004 * Eigen::Matrix3d::Identity();

    return true;
}

bool IMUGPS::Initializer::ComputeRotationFromImu(const Eigen::Matrix3d &G_R_I) {
    Eigen::Vector3d sum_acc(0.,0.,0.),sum_err2(0.,0.,0.);
    for(const auto& imu_data : imu_buffer)
    {
      sum_acc += imu_data->acc;
    }
    const Eigen::Vector3d mean_acc = sum_acc / imu_buffer.size();
    for(const auto& imu_data : imu_buffer)
    {
      sum_err2 += ((imu_data->acc - mean_acc) / imu_buffer.size()).cwiseSqrt();
    }

    Eigen::Vector3d z_axis = mean_acc.normalized();
    Eigen::Vector3d x_axis = Eigen::Vector3d::UnitX() - z_axis * z_axis.transpose() * Eigen::Vector3d::UnitX();
    x_axis.norm();
    Eigen::Vector3d y_axis = z_axis.cross(x_axis);
    y_axis.norm();

    Eigen::Vector3d I_R_G;
    I_R_G.block<3,1>(0,0) = x_axis;
    I_R_G.block<3,1>(0,1) = y_axis;
    I_R_G.block<3,1>(0,2) = z_axis;

    //G_R_I = I_R_G.transpose();
    return true;



}